#include <TString.h>
#include <TStopwatch.h>
#include <gsl/gsl_odeiv.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix_double.h>
#include <cmath>
#include "CpFunctions.h"
#include <CpGSLMain.h>
int run_test();
int func(double t, const double y[], double f[], void *params);
int jac(double t, const double y[], double *dfdy, double dfdt[], void *params);
int main(/*int argc, const char *argv[]*/)
{
   TStopwatch timer;
   timer.Start();
	Printf("Fun is %f",CpFunctions::dIp0(0.0,0.0,0.0,0.0,0.0,0.0));

	return 100;
   run_test();

   timer.Stop();
   timer.Print();
   return 0;
}

int run_test()
{
   const gsl_odeiv_step_type * T = gsl_odeiv_step_rk8pd;
   gsl_odeiv_step *s = gsl_odeiv_step_alloc(T, 6);
   gsl_odeiv_control *c = gsl_odeiv_control_y_new(1e-6, 0.0);
   gsl_odeiv_evolve *e = gsl_odeiv_evolve_alloc(6);

   // nase hustota krvi, polomer .. mu je nase Fmag

// toto
   double Fmagx = 1;
   double Fmagy = 1;
   double Fmagz = 1;
   double mp = 1;
   double a = 1;

//        double mu = 1;  //??? mu je nase mc?
   double mu [] = { Fmagx, Fmagy, Fmagz, mp, a, M_PI } ; //??? mu je nase mc?

   gsl_odeiv_system sys = {func, jac, 6, &mu} ;

   double t = 0.0, t1 = 100.0;
   double h = 1e-6;
   // x,y,z, rychlost vx, rychlost vy, rychlost vz,
   double Ip[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };

   for (int i = 1; i <= 100; i++) {
      double ti = i * t1 / 100.0;
      while (t < ti) {
			gsl_odeiv_evolve_apply(e, c, s, &sys, &t, ti, &h, Ip);
      }

      printf("%.5e %.5e %.5e %.5e %.5e %.5e %.5e\n", t, Ip[0], Ip[1], Ip[2], Ip[3], Ip[4], Ip[5]);
   }
   
   return 0;
}

int func(double t, const double Ip[], double f[], void */*params*/)
{
	// nech compilator nepinda (len docastne)
	if (0) { Printf("%5e",t);}

	// possible to take parameters using mu
// 	double *mu = (double *)params;

//        printf ("func mu : %.5e %.5e %.5e %.5e %.5e %.5e\n", mu[0], mu[1], mu[2], mu[3], mu[4], mu[5]);

   // TODO mozno to treba dat do mu parametrov
   // rychlost krvi
//    double vd1[] = {0.0, 0.0, 1.0};
   //  Riesenim DR II. radu je system 6 rovnic I. radu
   
  f[0] = CpFunctions::dIp0(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]);
  f[1] = CpFunctions::dIp1(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]);
  f[2] = CpFunctions::dIp2(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]);
  f[3] = CpFunctions::dIp3(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]);
  f[4] = CpFunctions::dIp4(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]);
  f[5] = CpFunctions::dIp5(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]);
  
   
  
   return GSL_SUCCESS;
}

int jac(double t, const double Ip[], double *dfdy, double dfdt[], void *params)
{
	
	// nech compilator nepinda (len docastne)
	if (0) { Printf("%5e %5e",t,Ip[0]);}
	
   double *mu = (double *)params;

   printf("jac mu : %.5e %.5e %.5e %.5e %.5e %.5e\n", mu[0], mu[1], mu[2], mu[3], mu[4], mu[5]);

   gsl_matrix_view dfdy_mat
   = gsl_matrix_view_array(dfdy, 6, 6);
   gsl_matrix * m = &dfdy_mat.matrix;
   // Jakobyjan robime parcialna derivacia fo/parcialna derivacia Y1; parcialna derivacia f0/parcialna derivacia Y2... 
	// prve 6 rovnci platia pre f0, dalsie 6 rovnice pre f1, ...

   gsl_matrix_set(m, 0, 0, CpFunctions::dF0D0(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 0, 1, CpFunctions::dF0D1(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 0, 2, CpFunctions::dF0D2(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 0, 3, CpFunctions::dF0D3(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 0, 4, CpFunctions::dF0D4(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 0, 5, CpFunctions::dF0D5(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 1, 0, CpFunctions::dF1D0(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 1, 1, CpFunctions::dF1D1(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 1, 2, CpFunctions::dF1D2(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 1, 3, CpFunctions::dF1D3(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 1, 4, CpFunctions::dF1D4(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 1, 5, CpFunctions::dF1D5(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 2, 0, CpFunctions::dF2D0(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 2, 1, CpFunctions::dF2D1(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 2, 2, CpFunctions::dF2D2(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 2, 3, CpFunctions::dF2D3(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 2, 4, CpFunctions::dF2D4(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 2, 5, CpFunctions::dF2D5(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 3, 0, CpFunctions::dF3D0(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 3, 1, CpFunctions::dF3D1(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 3, 2, CpFunctions::dF3D2(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 3, 3, CpFunctions::dF3D3(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 3, 4, CpFunctions::dF3D4(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 3, 5, CpFunctions::dF3D5(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 4, 0, CpFunctions::dF4D0(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 4, 1, CpFunctions::dF4D1(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 4, 2, CpFunctions::dF4D2(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 4, 3, CpFunctions::dF4D3(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 4, 4, CpFunctions::dF4D4(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 4, 5, CpFunctions::dF4D5(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 5, 0, CpFunctions::dF5D0(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 5, 1, CpFunctions::dF5D1(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 5, 2, CpFunctions::dF5D2(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 5, 3, CpFunctions::dF5D3(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 5, 4, CpFunctions::dF5D4(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));
   gsl_matrix_set(m, 5, 5, CpFunctions::dF5D5(Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]));

   dfdt[0] = CpFunctions::df0dt(t, Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]);
   dfdt[1] = CpFunctions::df1dt(t, Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]);
   dfdt[2] = CpFunctions::df2dt(t, Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]);
   dfdt[3] = CpFunctions::df3dt(t, Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]);
   dfdt[4] = CpFunctions::df4dt(t, Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]);
   dfdt[5] = CpFunctions::df5dt(t, Ip[0],Ip[1],Ip[2],Ip[3],Ip[4],Ip[5]);
   return GSL_SUCCESS;
}
